﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace RVO3D
{
    public struct Line
    {
        public Vector3 direction;  //直线的方向
        public Vector3 point;  //直线上的一点
    }


    public class Agent
    {
        
        internal Vector3 newVelocity_;
        internal Vector3 position_;
        internal Vector3 prefVelocity_; //参照速度
        internal Vector3 velocity_;

        //internal RVOSimulator sim_;

        internal int id_ = 0;
        internal int maxNeighbors_ = 0;

        internal float maxSpeed_ = 0;
        internal float neighborDist_ = 0;
        internal float radius_ = 0;
        internal float timeHorizon_ = 0;

        internal List<KeyValuePair<float, Agent>> agentNeighbors_ = new List<KeyValuePair<float, Agent>>();
        internal List<KeyValuePair<float, Obstacle>> obstacleNeighbors_ = new List<KeyValuePair<float, Obstacle>>();

        internal List<Plane> projPlanes = new List<Plane>();


        internal List<Plane> orcaPlanes_ = new List<Plane>();
        


        /// <summary>
        /// 计算这个Agent的邻居
        /// </summary>
        internal void ComputeNeighbors()
        {
            //Obstacle




            //Agent
            agentNeighbors_.Clear();
            if (maxNeighbors_ > 0)
            {
                RVOSimulator.Instance.kdTree_.ComputeAgentNeighbors(this, neighborDist_ * neighborDist_);
            }

            //for (int i = 0; i < agentNeighbors_.Count; i++)
            //{
            //    Debug.Log(agentNeighbors_[i].Value.id_);
            //}

        }

        /// <summary>
        /// 计算这个Agent的新速度
        /// </summary>
        internal void ComputeNewVelocity()
        {
            orcaPlanes_.Clear();
            float invTimeHorizon = 1.0f / timeHorizon_;

            for (int i = 0; i < agentNeighbors_.Count; i++)
            {
                Agent other = agentNeighbors_[i].Value;
                Vector3 relativePosition = other.position_ - position_;  //自身和附近物体相对位置
                Vector3 relativeVelocity = velocity_ - other.velocity_; //自身和附近Agent相对速度

                float distSq = Vector3.SqrMagnitude(relativePosition);
                    
                float combinedRadius = radius_ + other.radius_;  //计算躲避区域半径
                float combinedRadiusSq = Math.Square(combinedRadius); //躲避区域半径的平方

                Plane plane = new Plane();
                Vector3 u;

                //如果自身和目标的距离的平方大于躲避半径的平方
                if (distSq > combinedRadiusSq)
                {
                    Vector3 w = relativeVelocity - (relativePosition * invTimeHorizon);

                    float wLengthSq = Vector3.SqrMagnitude(w);
                    float dotProduct = Vector3.Dot(w, relativePosition);

                    if (dotProduct < 0.0f && Math.Square(dotProduct) > combinedRadius * wLengthSq)
                    {
                        float wLength = Mathf.Sqrt(wLengthSq);
                        Vector3 unitw = w / wLength;
                        plane.normal = unitw;
                        u = unitw * (combinedRadius * invTimeHorizon - wLength);
                    }
                    else
                    {
                        float a = distSq;
                        float b = Vector3.Dot(relativePosition, relativeVelocity);
                        float c =  Vector3.SqrMagnitude(relativeVelocity) - Vector3.SqrMagnitude( Vector3.Cross(relativePosition, relativeVelocity) )/ (distSq - combinedRadiusSq);
                        float t = (b + Mathf.Sqrt((Math.Square(b)) - a * c)) / a;
                        Vector3 ww = relativeVelocity - relativePosition * t;
                        float wwLength = Vector3.Magnitude(ww);
                        Vector3 unitWW = ww / wwLength;
                        plane.normal = unitWW;
                        u = unitWW * (combinedRadius * t - wwLength);
                    }
                }
                else
                {
                    float invTimeStep = 1.0f / RVOSimulator.Instance.timeStep_;
                    Vector3 w = relativeVelocity - ( relativePosition * invTimeStep);
                    float wLength = w.magnitude;
                    Vector3 unitW = w / wLength;
                    plane.normal = unitW;
                    u = unitW * (combinedRadius * invTimeStep - wLength);
                }
                plane.point = velocity_ + (u * 0.5f );
                orcaPlanes_.Add(plane);
            }

            int planeFail = LinearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
            if (planeFail < orcaPlanes_.Count)
            {
                LinearProgram4(orcaPlanes_, planeFail, maxSpeed_, ref newVelocity_);
            }
        }

        /// <summary>
        /// 代理的邻居集合中插入一个代理邻居
        /// </summary>
        /// <param name="agent">要插入的代理</param>
        /// <param name="rangeSq">这个代理周围的平方范围</param>
        internal void InsertAgentNeighbor(Agent agent, float rangeSq)
        {
            if (this != agent)
            {
                float distSq = Vector3.SqrMagnitude(position_ - agent.position_);

                if (distSq < rangeSq)
                {
                    if (agentNeighbors_.Count < maxNeighbors_)
                    {
                        agentNeighbors_.Add(new KeyValuePair<float, Agent>(distSq, agent));
                    }

                    int i = agentNeighbors_.Count - 1;

                    while (i != 0 && distSq < agentNeighbors_[i - 1].Key)
                    {
                        agentNeighbors_[i] = agentNeighbors_[i - 1];
                        --i;
                    }

                    agentNeighbors_[i] = new KeyValuePair<float, Agent>(distSq, agent);

                    if (agentNeighbors_.Count == maxNeighbors_)
                    {
                        rangeSq = agentNeighbors_[agentNeighbors_.Count - 1].Key;
                    }
                }
            }
        }

        /// <summary>
        /// 更新三维位置和三维速度
        /// </summary>
        public void Update()
        {
            velocity_ = newVelocity_;
            position_ += velocity_ * RVOSimulator.Instance.timeStep_;

            //Debug.Log(id_ + " " + velocity_);

        }


        private bool LinearProgram1(List<Plane> planes, int planeNo, Line line, float radius, Vector3 optVelocity, bool directionOpt, ref Vector3 result)
        {
            float dotProduct = Vector3.Dot(line.point , line.direction);
            float discriminant = Math.Square(dotProduct) + Math.Square(radius) - Vector3.SqrMagnitude(line.point);

            if (discriminant < 0.0f)
            {
                return false;
            }

            float sqrtDiscriminant = Mathf.Sqrt(discriminant);
            float tLeft = -dotProduct - sqrtDiscriminant;
            float tRight = -dotProduct + sqrtDiscriminant;

            for (int i = 0; i < planeNo; i++)
            {
                float numerator = Vector3.Dot((planes[i].point - line.point), planes[i].normal);
                float denominator = Vector3.Dot(line.direction, planes[i].normal);

                
                if (Math.Square(denominator) <= Math.RVO3D_EPSILON)
                {
                    ///line(几乎)平行于 平面i
                    if (numerator > 0f)
                    {
                        return false;
                    }
                    else
                    {
                        continue;
                    }
                }

                float t = numerator / denominator;

                if (denominator >= 0.0f)
                {
                    tLeft = Mathf.Max(tLeft, t);
                }
                else
                {
                    tRight = Mathf.Min(tRight, t);
                }

                if (tLeft > tRight)
                {
                    return false;
                }
            }

            if (directionOpt)
            {
                if (Vector3.Dot(optVelocity , line.direction) > 0.0f)
                {
                    result = line.point + (line.direction * tRight);
                }
                else
                {
                    result = line.point + (line.direction * tLeft);
                }
            }
            else
            {
                float t = Vector3.Dot(line.direction, (optVelocity - line.point));

                if (t < tLeft)
                {
                    result = line.point + (line.direction * tLeft);
                }
                else if(t > tRight)
                {
                    result = line.point + (line.direction * tRight);
                }
                else
                {
                    result = line.point + (line.direction * t);
                }
            }
            return true;
        }

        private bool LinearProgram2(List<Plane> planes, int planeNo, float radius, Vector3 optVelocity, bool directionOpt, ref Vector3 result)
        {
            float planeDist = Vector3.Dot(planes[planeNo].point, planes[planeNo].normal);
            float planeDisSq = Math.Square(planeDist);
            float radiusSq = Math.Square(radius);

            if (planeDisSq > radiusSq)
            {
                return false;
            }

            float planeRadiusSq = radiusSq - planeDisSq;

            Vector3 planeCenter = planeDist * planes[planeNo].normal;

            if (directionOpt)
            {
                Vector3 planeOptVelocity = optVelocity - Vector3.Dot(optVelocity, planes[planeNo].normal) * planes[planeNo].normal;
                float planeOptVelocityLengthSq = Vector3.SqrMagnitude(planeOptVelocity);

                if (planeOptVelocityLengthSq <= Math.RVO3D_EPSILON)
                {
                    result = planeCenter;
                }
                else
                {
                    result = planeCenter + Mathf.Sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity;
                }
            }
            else
            {
                result = optVelocity + Vector3.Dot((planes[planeNo].point - optVelocity) , planes[planeNo].normal) * planes[planeNo].normal;

                if (result.sqrMagnitude > radiusSq)
                {
                    Vector3 planeResult = result - planeCenter;
                    float planeResultLengthSq = planeResult.sqrMagnitude;
                    result = planeCenter + Mathf.Sqrt(planeRadiusSq / planeResultLengthSq) * planeResult;
                }
            }

            for (int i = 0; i < planeNo; i++)
            {
                if (Vector3.Dot(planes[i].normal , (planes[i].point - result)) > 0.0f)
                {
                    Vector3 crossProduct = Vector3.Cross(planes[i].normal, planes[planeNo].normal);
                    if (crossProduct.sqrMagnitude <= Math.RVO3D_EPSILON)
                    {
                        return false;
                    }

                    Line line = new Line();
                    line.direction = crossProduct.normalized;
                    Vector3 lineNormal = Vector3.Cross(line.direction, planes[planeNo].normal);

                    line.point = planes[planeNo].point + (Vector3.Dot((planes[i].point - planes[planeNo].point), planes[i].normal) / Vector3.Dot(lineNormal, planes[i].normal)) * lineNormal;

                    if (!LinearProgram1(planes, i, line, radius, optVelocity, directionOpt, ref result))
                    {
                        return false;
                    }
                }
            }
            return false;
        }

        private int LinearProgram3(List<Plane> planes, float radius, Vector3 optVelocity, bool directionOpt, ref Vector3 result)
        {
            if (directionOpt)
            {
                //优化方向。注意，在这种情况下，优化速度是单位长度
                result = optVelocity * radius;
            }
            else if (Vector3.SqrMagnitude(optVelocity) > Mathf.Sqrt(radius))
            {
                //优化最近点和外部圆
                result = optVelocity.normalized * radius;
            }
            else
            {
                result = optVelocity;
            }

            for (int i = 0; i < planes.Count; i++)
            {
                if (Vector3.Dot(planes[i].normal, (planes[i].point - result)) > 0.0f)
                {
                    Vector3 tempResult = result;
                    if (!LinearProgram2(planes,i,radius,optVelocity,directionOpt,ref result))
                    {
                        result = tempResult;
                        return i;
                    }
                }
            }
            return planes.Count;
        }

        private void LinearProgram4(List<Plane> planes, int beginPlane, float radius, ref Vector3 result)
        {
            float distance = 0.0f;

            for (int i = beginPlane; i < planes.Count; i++)
            {
                if (Vector3.Dot(planes[i].normal , (planes[i].point - result)) > distance)
                {
                    projPlanes.Clear();

                    for (int j = 0; j < i; j++)
                    {
                        Plane plane = new Plane();

                        Vector3 crossProduct = Vector3.Cross(planes[j].normal, planes[i].normal);

                        if (crossProduct.sqrMagnitude <= Math.RVO3D_EPSILON)
                        {
                            if (Vector3.Dot(planes[i].normal, planes[j].normal) > 0.0f)
                            {
                                continue;
                            }
                            else
                            {
                                plane.point = 0.5f * (planes[i].point + planes[j].point);
                            }
                        }
                        else
                        {
                            Vector3 lineNormal = Vector3.Cross(crossProduct, planes[i].normal);
                            plane.point = planes[i].point + Vector3.Dot((planes[j].point - planes[i].point), planes[j].normal) / Vector3.Dot(lineNormal, planes[j].normal) * lineNormal;
                        }

                        plane.normal = Vector3.Normalize(planes[j].normal - planes[i].normal);
                        projPlanes.Add(plane);
                    }

                    Vector3 tempResult = result;

                    if (LinearProgram3(projPlanes, radius,planes[i].normal, true, ref result) < projPlanes.Count)
                    {
                        result = tempResult;
                    }

                    distance = Vector3.Dot(planes[i].normal, planes[i].point - result);
                }
            }
        }
    }
}

